using namespace std;
using namespace cv;

class BeeHive
{
public:
	Conf conf;

	vector<Pair2i>* pairwiseMatches;
	const vector<KeyPoint>* keypoints1;
	const vector<KeyPoint>* keypoints2;
	
	int currentDance;
	int fieldSize;


	Outlier *biggestOutlier;
	
	Outlier **previousSolutions;

	vector<Outlier> outliers;
	vector<Outlier*> feasibleSolutions;
	vector<Outlier*> waggleList;

	double pFollowWaggleList;
	
public:

	BeeHive(vector<Pair2i>* _pairwiseMatches, const vector<KeyPoint>* _keypoints1,	const vector<KeyPoint>* _keypoints2);
	
	void init(int numberOfThreads, int population, double normDeviation, double suspectTresholdPct, double confirmTresholdPct)
	{
		this->fieldSize = this->pairwiseMatches->size();
		int suspectAttempts, confirmAttempts;
		if(this->fieldSize <=20)
		{
			suspectAttempts = 3;
			confirmAttempts = 10;
		}
		else if(this->fieldSize > 20 && this->fieldSize <=50)
		{
			suspectAttempts = 4;
			confirmAttempts = 15;
		}
		else
		{
			suspectAttempts = 5;
			confirmAttempts = 20;
		}

		init(numberOfThreads, population, normDeviation, suspectTresholdPct, confirmTresholdPct, suspectAttempts, confirmAttempts);
	}
	
	void init(int numberOfThreads, int population, double normDeviation, double suspectTresholdPct, double confirmTresholdPct, int suspectAttempts, int confirmAttempts)
	{
		this->fieldSize = this->pairwiseMatches->size();
		this->conf.NumberOfThreads = numberOfThreads;
		this->conf.Population = population;
		this->conf.NormDeviation = normDeviation;

		this->conf.OutlierSuspectAttempts = suspectAttempts;
		this->conf.OutlierConfirmAttempts = confirmAttempts;
		this->conf.OutlierSuspectTresholdPct = suspectTresholdPct;
		this->conf.OutlierConfirmTresholdPct = confirmTresholdPct;

		this->previousSolutions = new Outlier*[conf.Population];
		for(int i = 0; i < conf.Population; i++)
		{
			this->previousSolutions[i] = NULL;
		}
		this->biggestOutlier = NULL;
	}

	void destroy()
	{
		delete[] previousSolutions;
	}

	void generateInitialSolutions()
	{
		biggestOutlier = new Outlier();
		int scoutSearchSize = fieldSize / conf.Population;
		int module = fieldSize % conf.Population;
		int startIndex = 0;
		for(int i = 0; i < conf.Population; i++)
		{
			startIndex = i*scoutSearchSize;
			scout(i, startIndex, scoutSearchSize);
		}

		for(int i = 0; i < module; i++)
		{
			startIndex = i + conf.Population * scoutSearchSize; 
			scout(i, startIndex, 1);
		}
	}

	void exploreBigValey()
	{
		if(conf.NumberOfThreads > 1)
		{
			//goToWorkParallel();
		}
		else
		{
			goToWork();
		}
	}

	void goToWork()
	{
		generateInitialSolutions();
		
		if(biggestOutlier->suspectedAsOutlierCount > 0)
		{
			performWaggleDance(biggestOutlier);
		}
		else
		{
			return;
		}

		while(feasibleSolutions.size() > 0)
		{
			for(int foragingBee = 0; foragingBee < conf.Population; foragingBee++)
			{
				Outlier* followedOutlier = observeAndSelectDance(foragingBee);
				if(followedOutlier == NULL)
					continue;

				forage(followedOutlier);

				if(followedOutlier->suspectedAsOutlierPct() > biggestOutlier->suspectedAsOutlierPct())
				{
					performWaggleDance(followedOutlier);
					biggestOutlier = followedOutlier;
				}
			
				previousSolutions[foragingBee] = followedOutlier;
			}

			cleanupOutliers();

			if(pairwiseMatches->size() < conf.OutlierConfirmAttempts)
			{
				pairwiseMatches->clear();
				destroyOutliers();
			}
		}
	}

	void destroyOutliers()
	{
		for(int i = 0; i < conf.Population;i++)
		{
			previousSolutions[i] = NULL;
		}

		for(int i = feasibleSolutions.size() - 1; i > -1; i--)
		{
			Outlier* out = feasibleSolutions[i];
			feasibleSolutions.erase(feasibleSolutions.begin() + i);
			
			delete out;
		}
	}

	void cleanupOutliers()
	{
		vector<Outlier> tempConfirmedOutliers;

		for(int i = waggleList.size() - 1; i > -1; i--)
		{
			Outlier *out = waggleList[i];
			if(out->timesVisited >= conf.OutlierConfirmAttempts)
			{
				waggleList.erase(waggleList.begin() + i);
			}
		}

		for(int i = feasibleSolutions.size() - 1; i > -1; i--)
		{
			Outlier* out = feasibleSolutions[i];
			if(out->timesVisited >= conf.OutlierConfirmAttempts)
			{
				if(out->pair == biggestOutlier->pair)
				{
					biggestOutlier = getFeasibleSolution();
				}
				if(out->suspectedAsOutlierPct() >= conf.OutlierConfirmTresholdPct)
				{
					tempConfirmedOutliers.push_back(*out);
					outliers.push_back(*out);
				}
				feasibleSolutions.erase(feasibleSolutions.begin() + i);
				for(int i = 0; i < conf.Population;i++)
				{
					if(previousSolutions[i] == out)
					{
						previousSolutions[i] = NULL;
					}
				}
				delete out;
			}
		}

		if(tempConfirmedOutliers.size() > 0)
		{
			for(int j = 0; j < tempConfirmedOutliers.size(); j++)
			{
				for(int i = pairwiseMatches->size() - 1; i > -1; i--)
				{
					if(pairwiseMatches->at(i) == tempConfirmedOutliers.at(j).pair)
					{
						pairwiseMatches->erase(pairwiseMatches->begin() + i);
					}
				}
			}
			fieldSize = pairwiseMatches->size();

			tempConfirmedOutliers.clear();
		}
	}

	//void goToWorkParallel()
	//{
	//	/*omp_set_num_threads(conf.Number_Of_Threads);

	//	generateInitialSolutions();

	//	Solution* followedSolution;
	//	Solution newSolution;
	//	
	//	for(int i = 0; i < conf.NumberOfIterations; i++)
	//	{
	//		#pragma omp parallel for private(newSolution, followedSolution)
	//		for(int foragingBee = 0; foragingBee < conf.Population; foragingBee++)
	//		{
	//				followedSolution = observeAndSelectDance(foragingBee);
	//								
	//			
	//			newSolution = forage(*followedSolution);

	//			if(newSolution.iPenalty < bestSolution.iPenalty)
	//			{
	//				#pragma omp critical
	//				{
	//					if(newSolution.iPenalty < bestSolution.iPenalty)
	//					{
	//						performWaggleDance(&newSolution);
	//						bestSolution = newSolution;
	//					}
	//				}
	//			}
	//		
	//			previousSolutions[foragingBee] = newSolution;
	//		}
	//	}*/
	//}

	void performWaggleDance(Outlier *newSolution)
	{
		bool found = false;

		for(int i = 0; i < waggleList.size(); i++)
		{
			if(waggleList.at(i)->pair == newSolution->pair)
			{
				found = true;
				break;
			}
		}
				
		if(!found)
		{
			waggleList.push_back(newSolution);
		}

		double profitabilitySum = 0.0;
		for(int i = 0; i < waggleList.size(); i++)
		{
			profitabilitySum = profitabilitySum + (1.0 / ((double)waggleList.at(i)->suspectedAsOutlierPct()));
		}
		
		pFollowWaggleList = (double)profitabilitySum / (double)waggleList.size();
	}


	Outlier* observeAndSelectDance(int foragingBee)
	{
		Outlier *followedSolution = previousSolutions[foragingBee];
		
		if(followedSolution == NULL || followedSolution->timesVisited >= conf.OutlierConfirmAttempts)
		{
			followedSolution = getFeasibleSolution();
		}
		
		if(followedSolution != NULL) // in case there are no feasible solutions
		{
			double currentPenalty = (double)(followedSolution->suspectedAsOutlierPct());
			double pFollow = 1.0/currentPenalty;
			bool shouldFollow = pFollow > (1.2 * pFollowWaggleList);
			if(shouldFollow)
			{
				if(waggleList.size() > 0)
				{
					int randPeriod1 = rand() % 100;

					if(randPeriod1 <=60)
					{
						#pragma omp critical
						{
							Outlier* waggleSolution = getNextWaggleDanceSolution();
							if(waggleSolution->timesVisited < conf.OutlierConfirmAttempts)
							{
								followedSolution = waggleSolution;
							}
						}
					}
				}
			}
		}

		return followedSolution;
	}

	Outlier* getFeasibleSolution()
	{
		int size = feasibleSolutions.size();
		if(size == 0)
		{
			return NULL;
		}

		int idx = rand()%size;
		return feasibleSolutions.at(idx);
	}

	Outlier* getNextWaggleDanceSolution()
	{
		if(waggleList.size()<= currentDance)
		{
			currentDance = 0;
		}

		Outlier *next = waggleList.at(currentDance);

		currentDance++;
		return next;
	}


	void scout(int scoutNum, int startIndex, int scoutSearchSize)
	{
		int endIndex = startIndex + scoutSearchSize;
		for(int i = startIndex; i < endIndex; i++)
		{
			Outlier* out = new Outlier();
			lookForNecterHere(i, out);
			if(out->suspectedAsOutlierPct() >= conf.OutlierSuspectTresholdPct)
			{
				if(previousSolutions[scoutNum] == NULL || out->suspectedAsOutlierCount > previousSolutions[scoutNum]->suspectedAsOutlierCount)
				{
					previousSolutions[scoutNum] = out;
				}

				feasibleSolutions.push_back(out);

				if(out->suspectedAsOutlierCount > biggestOutlier->suspectedAsOutlierCount)
				{
					biggestOutlier = out;
				}
			}
			else
			{
				out->tabooList.clear();
				delete out;
			}
		}
	}

	void lookForNecterHere(int pairwiseMatch, Outlier* out)
	{
		Pair2i vec = pairwiseMatches->at(pairwiseMatch);
		Point2f first = keypoints1->at(vec.first).pt;
		Point2f second = keypoints2->at(vec.second).pt;

		double distance = norm(first - second);

		out->pair = vec;
		out->distance = distance;

		double tempDistance;
		int randomPairIdx;

		for(int i = 0; i < conf.OutlierSuspectAttempts; i++)
		{
			randomPairIdx = rand() % fieldSize;
			Pair2i randomVec = pairwiseMatches->at(randomPairIdx);
			
			int randomCount = 0;
			int randomCountMax = fieldSize*2;
			while(std::find(out->tabooList.begin(),out->tabooList.end(),randomVec) != out->tabooList.end())
			{
				if(randomCount > randomCountMax)
					break;
				randomPairIdx = rand() % fieldSize;
				randomVec = pairwiseMatches->at(randomPairIdx);

				randomCount++;
			}
			
			out->tabooList.push_back(randomVec);
			out->timesVisited++;
			
			first = keypoints1->at(randomVec.first).pt;
			second = keypoints2->at(randomVec.second).pt;
			tempDistance = norm(first - second);
			if(fabs(distance - tempDistance) > conf.NormDeviation)
			{
				out->suspectedAsOutlierCount++;
			}
		}

	}

	void forage(Outlier* out) 
	{
		int randomPairIdx = rand() % fieldSize;
		Pair2i randomVec = pairwiseMatches->at(randomPairIdx);
	
		int randomCount = 0;
		int randomCountMax = fieldSize*2;
		while(std::find(out->tabooList.begin(),out->tabooList.end(),randomVec) != out->tabooList.end())
		{
			if(randomCount > randomCountMax)
				break;
			randomPairIdx = rand() % fieldSize;
			randomVec = pairwiseMatches->at(randomPairIdx);

			randomCount++;
		}

		out->tabooList.push_back(randomVec);
		out->timesVisited++;
		
		Point2f first = keypoints1->at(randomVec.first).pt;
		Point2f second = keypoints2->at(randomVec.second).pt;
		double tempDistance = norm(first - second);
		if(fabs(out->distance - tempDistance) > conf.NormDeviation)
		{
			out->suspectedAsOutlierCount++;
		}
	}
};

BeeHive::BeeHive(vector<Pair2i>* _pairwiseMatches, const vector<KeyPoint>* _keypoints1,	const vector<KeyPoint>* _keypoints2)
{
	this->pairwiseMatches = _pairwiseMatches;
	this->keypoints1 = _keypoints1;
	this->keypoints2 = _keypoints2;
}